#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy                                     # ROS2 Python接口库
from rclpy.node import Node                      # ROS2 节点类
from std_msgs.msg import String                  # 字符串消息类型

import threading                                 # 多线程
from flask import Flask, Blueprint, jsonify      # Flask
from flask_sock import Sock                      # Sock

def sendOK(data):
    return jsonify({'code': 200, 'message': 'ok', 'data': data}), 200

def sendError(message):
    return jsonify({'code': 500, 'message': message, 'data': None}), 500

# 创建app
app = Flask(__name__)
app_sock = Sock(app)

# 创建一个蓝图，指定静态文件夹
ros_api_blueprint = Blueprint('ros-api', __name__, url_prefix='/ros-api')
ros_api_sock = Sock(ros_api_blueprint)

# ros 全局node变量
node = None

# 定义HTTP服务接口
@ros_api_blueprint.route('/send_message', methods=['GET'])
def send_message():
    msg = String()                                            # 创建一个String类型的消息对象
    msg.data = 'Hello send_message'                           # 填充消息对象中的消息数据
    node.chatter_pub.publish(msg)                             # 发布话题消息
    node.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布
    return sendOK(None)

@app_sock.route('/ros-ws')
def ros_ws(ws):
    while True:
        data = ws.receive()
        if data == 'close':
            break
        msg = String()                                            # 创建一个String类型的消息对象
        msg.data = data                                           # 填充消息对象中的消息数据
        node.chatter_pub.publish(msg)                             # 发布话题消息
        node.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布
        ws.send(data)

@ros_api_sock.route('/sock')
def sock(ws):
    while True:
        data = ws.receive()
        if data == 'close':
            break
        msg = String()                                            # 创建一个String类型的消息对象
        msg.data = data                                           # 填充消息对象中的消息数据
        node.chatter_pub.publish(msg)                             # 发布话题消息
        node.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布
        ws.send(data)

# 注册蓝图
app.register_blueprint(ros_api_blueprint)

# run app
def run_app():
    # 启动Flask HTTP服务
    app.run(host='0.0.0.0', port=10008)

"""
创建一个http到ros的桥节点 
"""
class HttpRosBridgeNode(Node):
    def __init__(self, name):
        super().__init__(name)                                    # ROS2节点父类初始化
        self.chatter_pub = self.create_publisher(String, "chatter", 10)   # 创建发布者对象（消息类型、话题名、队列长度）
        self.timer = self.create_timer(0.5, self.timer_callback)  # 创建一个定时器（单位为秒的周期，定时执行的回调函数）get_logger
        
    def timer_callback(self):                                     # 创建定时器周期执行的回调函数
        msg = String()                                            # 创建一个String类型的消息对象
        msg.data = 'Hello World'                                  # 填充消息对象中的消息数据
        self.chatter_pub.publish(msg)                             # 发布话题消息
        self.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布

def main(args=None):                                 # ROS2节点主入口main函数
    # 在后台线程中初始化app
    ros_thread = threading.Thread(target=run_app)
    ros_thread.daemon = True
    ros_thread.start()

    # 初始化node
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    global node
    node = HttpRosBridgeNode("http_ros_bridge")      # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口
